#include <lversion.h>
#if L_VERSION_MODULE_TRIANGLE == 111225

#include "_ltriangle_111225.h"

using namespace llib::geom;
using namespace llib::math;

// static triangle-triangle intersection test
// [Tomas Moller 97] : "A Fast Triangle-Triangle Intersection Test"
llib::Boolean TriPairTester::TestStatic( Tri3f& firstLoc, Tri3f& secondLoc )
{
    // plane equation donation: A*x + B*y + C*z + D = 0
    //Float A, B, C, D;

    // step 1. "Compute plane equation of triangle 2"
    Vec3f v_2_01 = secondLoc.cell[1] - secondLoc.cell[0];
    Vec3f v_2_02 = secondLoc.cell[2] - secondLoc.cell[0];
    // (A, B, C) == n_2
    Vec3f n_2 = v_2_01.Cross(v_2_02);
    // D == D_2
    Float D_2 = -(n_2 * secondLoc.cell[0]);
    Float d_1_0 = firstLoc.cell[0] * n_2 + D_2;
    Float d_1_1 = firstLoc.cell[1] * n_2 + D_2;
    Float d_1_2 = firstLoc.cell[2] * n_2 + D_2;

    // coplanar check
    if(abs(d_1_0) < EPS_F && abs(d_1_1) < EPS_F && abs(d_1_2) < EPS_F) {
        Tri2f firstT, secondT;
        Vec3f abs_n_2;
        FOR(I,0,3) abs_n_2.cell[I] = abs(n_2.cell[I]);
        Int32 omitAxis = -1;	// 0:x, 1:y, 2:z
        if(abs_n_2.x > abs_n_2.y) {
            if(abs_n_2.x > abs_n_2.z)	omitAxis = 0;
            else				        omitAxis = 2;
        } else {
            if(abs_n_2.y > abs_n_2.z)	omitAxis = 1;
            else				        omitAxis = 2;
        }
        const static int selectAxis[3][2] = { {1,2}, {0,2}, {0,1} };
        FOR(I,0,3) {
            firstT.cell[I].cell[0] = firstLoc.cell[I].cell[ selectAxis[omitAxis][0] ];
            firstT.cell[I].cell[1] = firstLoc.cell[I].cell[ selectAxis[omitAxis][1] ];
            secondT.cell[I].cell[0] = secondLoc.cell[I].cell[ selectAxis[omitAxis][0] ];
            secondT.cell[I].cell[1] = secondLoc.cell[I].cell[ selectAxis[omitAxis][1] ];
        }
        return TriangleTriangleIntersection_2D(firstT, secondT);
    }

    // step 2. "Reject as trivial if all points of triangle 1 are on same side"
    Uint8 flag_1 = 0;
    if(d_1_0 > 0) flag_1 += 1 << 0;
    if(d_1_1 > 0) flag_1 += 1 << 1;
    if(d_1_2 > 0) flag_1 += 1 << 2;
    if(flag_1 == 0 || flag_1 == 7)
        return FALSE;

    // step 3. "Compute plane equation of triangle 1"
    Vec3f v_1_01 = firstLoc.cell[1] - firstLoc.cell[0];
    Vec3f v_1_02 = firstLoc.cell[2] - firstLoc.cell[0];
    // (A, B, C) == n_1
    Vec3f n_1 = v_1_01.Cross(v_1_02);
    // D == D_1
    Float D_1 = -(n_1 * firstLoc.cell[0]);
    Float d_2_0 = secondLoc.cell[0] * n_1 + D_1;
    Float d_2_1 = secondLoc.cell[1] * n_1 + D_1;
    Float d_2_2 = secondLoc.cell[2] * n_1 + D_1;

    // step 4. "Reject as trivial if all points of triangle 2 are on same side"
    Uint8 flag_2 = 0;
    if(d_2_0 > 0) flag_2 += 1 << 0;
    if(d_2_1 > 0) flag_2 += 1 << 1;
    if(d_2_2 > 0) flag_2 += 1 << 2;
    if(flag_2 == 0 || flag_2 == 7)
        return FALSE;

    // step 5. "Compute intersection line and project onto largest axis"
    // step 6. "Compute the intervals for each triangle"
    Vec3f D = n_1.Cross(n_2);
    FOR(I,0,3) D.cell[I] = abs(D.cell[I]);	// bug fixed at 2011-10-8
    // here comes the optimization strategy on paper
    Uint8 D_max_axis = 0;
    if(D.x > D.y) {
        if(D.x > D.z)	D_max_axis = 0;
        else			D_max_axis = 2;
    } else {
        if(D.y > D.z)	D_max_axis = 1;
        else			D_max_axis = 2;
    }
    const static Boolean positiveSingleTable[6] = { TRUE, TRUE, FALSE, TRUE, FALSE, FALSE };
    // arrange T1,T2's vertexes so that v1 is seperated with v0 & v2, by L
    Boolean positiveSingle;
    Vec3f *p_0, *p_1, *p_2;
    Float   *pd_0, *pd_1, *pd_2;
    // T1
    positiveSingle = positiveSingleTable[flag_1 - 1];
    if(d_1_0 > 0 == positiveSingle) {
        p_0 = &firstLoc.cell[1];
        p_1 = &firstLoc.cell[0];
        p_2 = &firstLoc.cell[2];
        pd_0 = &d_1_1;
        pd_1 = &d_1_0;
        pd_2 = &d_1_2;
    } else if(d_1_1 > 0 == positiveSingle) {
        p_0 = &firstLoc.cell[0];
        p_1 = &firstLoc.cell[1];
        p_2 = &firstLoc.cell[2];
        pd_0 = &d_1_0;
        pd_1 = &d_1_1;
        pd_2 = &d_1_2;
    } else {
        p_0 = &firstLoc.cell[0];
        p_1 = &firstLoc.cell[2];
        p_2 = &firstLoc.cell[1];
        pd_0 = &d_1_0;
        pd_1 = &d_1_2;
        pd_2 = &d_1_1;
    }
    Float pv_1_0 = p_0->cell[D_max_axis];
    Float pv_1_1 = p_1->cell[D_max_axis];
    Float pv_1_2 = p_2->cell[D_max_axis];
    Float t_1_1 = pv_1_0 + (pv_1_1 - pv_1_0) * (*pd_0 / (*pd_0 - *pd_1));
    Float t_1_2 = pv_1_2 + (pv_1_1 - pv_1_2) * (*pd_2 / (*pd_2 - *pd_1));
    // T2
    positiveSingle = positiveSingleTable[flag_2 - 1];
    if(d_2_0 > 0 == positiveSingle) {
        p_0 = &secondLoc.cell[1];
        p_1 = &secondLoc.cell[0];
        p_2 = &secondLoc.cell[2];
        pd_0 = &d_2_1;
        pd_1 = &d_2_0;
        pd_2 = &d_2_2;
    } else if(d_2_1 > 0 == positiveSingle) {
        p_0 = &secondLoc.cell[0];
        p_1 = &secondLoc.cell[1];
        p_2 = &secondLoc.cell[2];
        pd_0 = &d_2_0;
        pd_1 = &d_2_1;
        pd_2 = &d_2_2;
    } else {
        p_0 = &secondLoc.cell[0];
        p_1 = &secondLoc.cell[2];
        p_2 = &secondLoc.cell[1];
        pd_0 = &d_2_0;
        pd_1 = &d_2_2;
        pd_2 = &d_2_1;
    }
    Float pv_2_0 = p_0->cell[D_max_axis];
    Float pv_2_1 = p_1->cell[D_max_axis];
    Float pv_2_2 = p_2->cell[D_max_axis];
    Float t_2_1 = pv_2_0 + (pv_2_1 - pv_2_0) * (*pd_0 / (*pd_0 - *pd_1));
    Float t_2_2 = pv_2_2 + (pv_2_1 - pv_2_2) * (*pd_2 / (*pd_2 - *pd_1));

    // step 7. "Intersect the intervals"
    Float t_1_max = t_1_1, t_1_min = t_1_2;
    if(t_1_max < t_1_min) std::swap(t_1_max, t_1_min);
    Float t_2_max = t_2_1, t_2_min = t_2_2;
    if(t_2_max < t_2_min) std::swap(t_2_max, t_2_min);

    if(t_1_max < t_2_min) return FALSE;
    if(t_2_max < t_1_min) return FALSE;

    return TRUE;
}

// intersection test for two triangle on plane
llib::Boolean TriPairTester::TriangleTriangleIntersection_2D( Tri2f& t1, Tri2f& t2 )
{
    FOR(I,0,3)
        if(PointInsideTriangle_2D(t1,t2[I]))
            return TRUE;
    return FALSE;
}

llib::Boolean TriPairTester::PointInsideTriangle_2D( Tri2f& t, Vec2f& p )
{
    // barycentric coordinates to determine
    Vec2f v0 = t[1] - t[0];
    Vec2f v1 = t[2] - t[0];
    Vec2f v2 = p - t[0];

    Float d00 = v0 * v0;
    Float d01 = v0 * v1;
    Float d11 = v1 * v1;
    Float d20 = v2 * v0;
    Float d21 = v2 * v1;
    Float denom = d00 * d11 - d01 * d01;

    Float v = (d11 * d20 - d01 * d21) / denom;
    if(v < 0 || v > 1) return FALSE;
    Float w = (d00 * d21 - d01 * d20) / denom;
    if(w < 0 || w > 1) return FALSE;
    Float u = 1.0f - v - w;
    if(u < 0 || u > 1) return FALSE;
    return TRUE;
}

#endif
